/*
 * @Author: YuanQuan D
 * @Date: 2025-03-19 07:38:47
 * @LastEditTime: 2025-07-18 15:26:37
 * @FilePath: \trunk\User_Code\Src\motor_control.c
 */
#include "motor_control.h"

extern RC_channelTypedef RC_channel_1;
extern RC_channelTypedef RC_channel_2;
extern RC_channelTypedef RC_channel_3;
extern RC_channelTypedef RC_channel_4;
extern RC_channelTypedef RC_channel_5;
extern RC_channelTypedef RC_channel_6;

__IO uint32_t g_set_speed  = 8;          /* 最大速度 单位为0.1rad/sec */
__IO uint32_t g_step_accel = 8;          /* 加速度 单位为0.1rad/sec^2 */
__IO uint32_t g_step_decel = 8;          /* 减速度 单位为0.1rad/sec^2 */
__IO uint16_t g_step_angle = 5000;       /* 设置的步数*/

extern speedRampData g_srd1;
extern speedRampData g_srd2;
extern speedRampData g_srd3;
extern speedRampData g_srd4;

uint8_t key3_state = 0;
uint8_t key4_state = 0;

//1\2\3\4  8.9 10.7 12.5
//5\6 7.1 10.7 14.2

void Button_Control(void)
{
    // key3_state = HAL_GPIO_ReadPin(KEY_3_GPIO_Port, KEY_3_Pin);
    // key4_state = HAL_GPIO_ReadPin(KEY_4_GPIO_Port, KEY_4_Pin);
    // if(key3_state == RESET) 
    // {
    //     step1_create_t_ctrl_param(SPR*g_step_angle, g_step_accel, g_step_decel, g_set_speed);
    //     step2_create_t_ctrl_param(SPR*g_step_angle, g_step_accel, g_step_decel, g_set_speed);
    //     step3_create_t_ctrl_param(SPR*g_step_angle, g_step_accel, g_step_decel, g_set_speed);
    //     step4_create_t_ctrl_param(SPR*g_step_angle, g_step_accel, g_step_decel, g_set_speed);
    // }
    // else if(key4_state == RESET)
    // {
    //     step1_create_t_ctrl_param(-SPR*g_step_angle, g_step_accel, g_step_decel, g_set_speed);
    //     step2_create_t_ctrl_param(-SPR*g_step_angle, g_step_accel, g_step_decel, g_set_speed);
    //     step3_create_t_ctrl_param(-SPR*g_step_angle, g_step_accel, g_step_decel, g_set_speed);
    //     step4_create_t_ctrl_param(-SPR*g_step_angle, g_step_accel, g_step_decel, g_set_speed);
    // }
    // else
    // {
    //     g_srd1.run_state = STOP; 
    //     g_srd2.run_state = STOP; 
    //     g_srd3.run_state = STOP; 
    //     g_srd4.run_state = STOP; 
    // }
}


void MControl(void)
{
    
    if(RC_channel_2.duty_cycle >= 12 && RC_channel_2.duty_cycle <= 13)
    {
        step4_create_t_ctrl_param(SPR*g_step_angle, g_step_accel, g_step_decel, g_set_speed);
    }
    else if(RC_channel_2.duty_cycle >= 8.5 && RC_channel_2.duty_cycle <= 9.5) 
    {
        step4_create_t_ctrl_param(-SPR*g_step_angle, g_step_accel, g_step_decel, g_set_speed);
    }
    else
    {
        g_srd4.run_state = STOP; 
    }

    if(RC_channel_4.duty_cycle >= 12 && RC_channel_4.duty_cycle <= 13)
    {
        step3_create_t_ctrl_param(SPR*g_step_angle, g_step_accel, g_step_decel, g_set_speed);
    }
    else if(RC_channel_4.duty_cycle >= 8.5 && RC_channel_4.duty_cycle <= 9.5) 
    {
        step3_create_t_ctrl_param(-SPR*g_step_angle, g_step_accel, g_step_decel, g_set_speed);
    }
    else
    {
        g_srd3.run_state = STOP; 
    }

    if(RC_channel_5.duty_cycle >= 13.5 && RC_channel_5.duty_cycle <= 14.5)
    {
        step1_create_t_ctrl_param(SPR*g_step_angle, g_step_accel/4, g_step_decel/4, g_set_speed/4);
    }
    else if(RC_channel_5.duty_cycle >= 6.5 && RC_channel_5.duty_cycle <= 7.5)
    {
        step1_create_t_ctrl_param(-SPR*g_step_angle, g_step_accel/4, g_step_decel/4, g_set_speed/4);
    }
    else
    {
        g_srd1.run_state = STOP; 
    }
}






